from ipaddress import v4_int_to_packed
import numpy as np
import math
import cv2
from math import sin, cos, pi

def TransMatrix(shit : list) -> list:
    trans = []
    x = shit[0]
    y = shit[1]
    z = shit[2]
    alpha = shit[3]
    theta = shit[4]
    beta = shit[5]
    a1 = cos(theta)*cos(beta)
    a2 = sin(alpha)*sin(theta)*cos(beta)-cos(alpha)*sin(beta)
    a3 = cos(alpha)*sin(theta)*cos(beta)+sin(alpha)*sin(beta)
    a4 = x
    a5 = cos(theta)*cos(beta)
    a6 = sin(alpha)*sin(theta)*sin(beta)+cos(alpha)*sin(beta)
    a7 = cos(alpha)*sin(theta)*sin(beta)-sin(alpha)*cos(beta)
    a8 = y
    a9 = -sin(theta)
    a10 = sin(alpha)*cos(theta)
    a11 = cos(alpha)*cos(theta)
    a12 = z
    trans = np.array([[a1,a2,a3,a4],[a5,a6,a7,a8],[a9,a10,a11,a12],[0,0,0,1]])
    return trans

Matrix = [  [-0.99571636,0.09214242,0.0076614,0.62653163],
            [0.09237527,0.99493334,0.0396794,0.24948003],
            [-0.00396642,0.04021715,-0.99918309,0.84846713],
            [ 0.,0.,0.,1.,]]

command = [76,75,509,9,-2,-1]
# command 前6个是坐标，后五个是电机
x, y, z = float(command[0])/1000.0, float(command[1])/1000.0, float(command[2])/1000.0
rx, ry, rz = float(command[3])/10, float(command[4])/10, float(command[5])/10

Object2C = TransMatrix([x,y,z,rx,ry,rz])
object2base = np.dot(Matrix, Object2C)
tcprxryrz = cv2.Rodrigues(object2base[:3,:3])[0]
print ([object2base[0][3], object2base[1][3], object2base[2][3], tcprxryrz[0][0], tcprxryrz[1], tcprxryrz[2]])